#pragma once
#include "ft_interfaces.h"
#include "sys_thread.hpp"

class kuka_ft: public FTInterfaces
{
    private:
        /* data */
        kuka_ft(/* args */);
        ~kuka_ft();
        double *_force;
        double *_torque;
        double *_force_old;
        double *_torque_old;
        int _stall_counts;
    public:
    int getWrenchSensor(double *wrench);
    /**
     * Get the wrench in tool frame.
     */
    int getWrenchTool(double *wrench);
    /**
     * Get the tool wrench after tool weight compensation.
     */
    int getWrenchNetTool(const double *pose, double *wrench_net_T);

    void getRealSensor();
    void getVrepSensor();
    void init_param();

    static kuka_ft &getInstance(){
        static kuka_ft instance;
        return instance;   
    }
};


